#Copyright (c) 2020 Ocado. All Rights Reserved.

import random, skimage
from PIL import Image
import numpy as np
from skimage.filters import threshold_mean


class GridEnviroment:
    """
    2D environment represented as a discrete grid of empty/obstacle cells
    二维环境表示为空/障碍单元的离散网格
    """
    def __init__(self, grid):
        """
        :param grid: Binary matrix representing the environment, with 0 = free & 1 = obstacle
        :param grid: 表示环境的二进制矩阵，0 = free & 1 = obstacle
        grid是一个图片二值化后的矩阵，只有0，1
        """
        self.grid = grid
        self.dimensions = 2
        nrows, ncols = self.grid.shape
        self.bounds = np.array([[0,ncols], [0,nrows]])#一个包含两个元素的numpy数组[[0 4]，[0 3]]
        self.freeness = 1-self.grid.mean()

    @classmethod
    def from_image(cls, image_name):
        """
        Load grid environment from bitmap image by thresholding, setting light pixels as free cells and dark as obstacles

        :param image_name: File name of image to load grid from

        通过阈值处理从位图图像加载网格环境，将亮像素设置为自由单元，将暗像素设置为障碍物

         ：param image_name：要从中加载网格的图像的文件名
        """
        image = Image.open(image_name).convert("L") #8位像素，黑白
        grid = np.asarray(image).copy() #转化成矩阵
        #自己二值化
        threshold = threshold_mean(grid)
        grid[grid < threshold] = 1
        grid[grid >= threshold] = 0
        return cls(grid) #调用自己的初始化函数，来返回一个 和自己相同的类型

    def free_point(self, point):
        """
        Check if given point is in free space
        """
        col, row = map(int, point)
        if 0 <= row < self.grid.shape[0] and 0 <= col < self.grid.shape[1]:
            return self.grid[row, col] == 0
        return False

    def intersections(self, a, b):
        """
        Return set of grid positions on the line between positions a and b that are occupied by obstacles
        返回在位置a和位置b之间的线路上被 障碍物 占据的网格位置集合。
        """
        ac, ar = map(int, np.maximum(self.bounds[:,0], np.minimum(a, self.bounds[:,1]-1)))
        '''
        self.bounds = np.array([[0, 100], [1, 50]])
        这里，搜索空间的第一维的取值范围是 $[0, 100]$，第二维的取值范围是 $[1, 50]$。
        self.bounds是搜索空间，self.bounds[:,1]是上界，self.bounds[:,0]是下界
        
        接下来，np.minimum(a, self.bounds[:,1]-1) 会将 a 中的每个元素与 self.bounds 中每个元素的第二个值（即上界）作比较，将较小的值作为输出。
        其中，a 是另一个形状为 (n_dim,) 的 NumPy 数组，表示当前的搜索点。这一步的目的是，如果搜索点中有某个维度的值超出了搜索空间的上界，
        那么将该维度的值设置为上界减 1。这里减去 1 是因为 Python 中下标是从 0 开始的，所以边界上的值应该被认为是已经包含在搜索空间内的。
        
        然后，np.maximum(self.bounds[:,0], ...) 将 self.bounds 中每个元素的第一个值（即下界）与上面得到的数组作比较，将较大的值作为输出。
        这里的目的是确保搜索点中每个维度的值都 不小于 搜索空间的下界。
        
        最后，ac, ar = map(int, ...) 将上面得到的数组转换成整数类型，并分别赋值给变量 ac 和 ar。这里使用了 Python 的内置函数 map，
        它将一个函数应用于一个可迭代对象的每个元素，并返回一个迭代器。在这里，map(int, ...) 的作用是将数组中的每个元素转换成整数。
        因为数组的形状为 (n_dim, 2)，所以最终得到的 ac 和 ar 都是长度为 n_dim 的整数数组，它们分别表示了搜索点在每个维度上取值的下标。
        '''
        bc, br = map(int, np.maximum(self.bounds[:,0], np.minimum(b, self.bounds[:,1]-1)))
        rs, cs, _ = skimage.draw.line_aa(ar, ac, br, bc)
        '''
        draw.line_aa 函数的输入参数有四个：起点坐标 (r0, c0) 和终点坐标 (r1, c1)。它的返回值是一个元组 (rr, cc, val)：

        rr, cc 是经过反锯齿算法计算得到的线条的 坐标数组 。
        val 是经过反锯齿算法计算得到的线条的颜色值。默认为 1.0。
        '''
        args = np.nonzero(self.grid[rs, cs])
        # 线段上的非零点，也就是路径上的障碍物
        return np.column_stack((cs[args], rs[args]))
    #将所有在线条上的像素点的坐标组合成一个坐标矩阵并返回

    def closest_intersection(self, a, b):
        """
        Return intersection closest to position a of those on the line between a and b, or None if none exist
        这个函数返回在线段ab上距 离点a最近的交点 ，或者如果不存在交点则返回None。
        """
        ints = self.intersections(a, b)
        if len(ints) == 0:
            return None
        return ints[0]

    def area(self):
        """
        Return the total free area in the environment
        """
        dimensions = [bound[1] - bound[0] for bound in self.bounds]
        return np.prod(dimensions) * self.freeness

    def display_image(self):
        """
        Return PIL image representing the grid, with free cells represented by a white pixel and occupied cells by black
        返回表示网格的PIL图像，自由单元格由白色像素表示，占用单元格由黑色表示
        """
        image_grid = self.grid.copy()
        image_grid[image_grid == 0] = 255
        image_grid[image_grid == 1] = 0
        return Image.fromarray(image_grid).convert("RGB")


class StateSpace:
    """
    Generic state space class with an internal environment that can be queried for free spaces and obstacles
    具有可查询自由空间和障碍的内部环境的通用状态空间类
    """
    def __init__(self, env):
        """
        :param env: Environment the state space can interect with
        Param env: 状态空间可以与之交互的环境
        #env是一个GridEnviroment类，什么dimensions，bounds；GridEnviroment
        """
        self.env = env
        self.dimensions = self.env.dimensions
        self.bounds = self.env.bounds
        self.dynamic_obstacles = []  #创建一个集合

    @classmethod
    def from_image(cls, image_name):
        """
        Load state space from image, using 2D grid environment
        使用二维网格环境从图像加载状态空间
        """
        return cls(GridEnviroment.from_image(image_name))
    #GridEnviroment.from_image返回一个GridEnviroment类
    #这里再调用自己的构造函数(输入参数为GridEnviroment，env)，返回一个StateSpace类

    def choose_state_uniform(self):
        """
        Randomly sample free state in the space uniformly
        """
        while True:
            pos = np.random.uniform(self.env.bounds[:,0], self.env.bounds[:,1])
            if self.free_position(pos):
                return self.create_state(pos)

    def choose_state_line(self, a, b):
        """
        Randomly (uniformly) sample free state on the line between a and b
        """
        while True:
            p = np.random.rand()
            pos = (1-p)*a + p*b
            if self.free_position(pos):
                return self.create_state(pos)

    def choose_state_ellipse(self, a, b, major, minor):
        """
        Randomly (uniformly) sample free state in the defined ellipse (for 2D spaces)

        :param a: Position of first focus of the ellipse
        :param b: Position of second focus of the ellipse
        :param major: Length of major axis
        :param minor: Length of minor axis
        """
        if major == np.inf or minor == np.inf:
            return self.choose_state_uniform()
        while True:
            r = np.sqrt(np.random.rand())
            t = np.random.rand() * 2 * np.pi
            rot = np.arctan2((b-a)[1], (b-a)[0])
            xe = major * r * np.cos(t)
            ye = minor * r * np.sin(t)
            xr = xe * np.cos(rot) - ye * np.sin(rot)
            yr = xe * np.sin(rot) + ye * np.cos(rot)
            pos = np.array([xr,yr]) + 0.5*(a+b)
            if self.free_position(pos):
                return self.create_state(pos)

    def create_state(self, pos):
        """
        Create state object from given position
        将给定的位置创建状态对象。
        """
        return State(pos)

    def free_position(self, pos):
        """
        Check if given position is in free space
        """
        if not self.dynamically_free_position(pos):
            return False
        for i in range(self.dimensions):
            if not (self.env.bounds[i][0] <= pos[i] <= self.env.bounds[i][1]):
                return False
        return self.env.free_point(pos)

    def free_path(self, start, end):
        """
        Check if the line between start and end is obstacle free
        """
        if not self.dynamically_free_path(start, end):
            return False
        if not self.free_position(start.pos) or not self.free_position(end.pos):
            return False
        return len(self.env.intersections(start.pos, end.pos)) == 0

    def dynamically_free_position(self, pos):
        """
        Check if given position is free from dynamic obstacles
        """
        for obstacle in self.dynamic_obstacles:
            if not obstacle.free_position(pos) : return False
        return True

    def dynamically_free_path(self, start, end):
        """
        Check if the line between start and end is free from dynamic obstacles
        检查起点和终点之间的连线是否没有被动态障碍物阻挡。
        """
        for obstacle in self.dynamic_obstacles:
            if not obstacle.free_path(start, end) : return False
        return True

    def area(self):
        """
        Return the total free area of the state space
        """
        return self.env.area()

    def display_image(self):
        """
        Return a PIL image representing the state space
        """
        return self.env.display_image()

    def add_dynamic_obstacle(self, pos, radius,Obs_num):
        """
        Add circular dynamic obstacle

        :param pos: Position of dynamic obstacle
        :param radius: Radius of dynamic obstacle
        """
        self.dynamic_obstacles.append(DynamicObstacle(pos, radius,Obs_num))


class State:
    """
    Class encapsulating a single state in the state space
    """
    def __init__(self, pos):
        """
        :param pos: Position that the state represents
        """
        self.pos = np.array(pos, dtype="float")

    def __eq__(self, other):
        if isinstance(other, State):
            return (self.pos == other.pos).all()
        return False

    def __hash__(self):
        return hash(tuple(self.pos))

    def __lt__(self, other):
        return len(self.pos) < len(other.pos)


class DynamicObstacle:
    def __init__(self, pos, radius,Obs_num):
        self.pos = pos
        self.radius = radius
        self.Obs_num=Obs_num

    def free_position(self, pos):
        """
        Check that the given pos is not within the obstacle
        """
        return np.linalg.norm(pos-self.pos) > self.radius

    def free_path(self, start, end):
        """
        Check if the line between start and end does not intersect the obstacle
        """
        a = start.pos[1] - end.pos[1]
        b = end.pos[0] - start.pos[0]
        c = -(a*start.pos[0] + b*start.pos[1])

        """
        在方法中，首先根据起点和终点计算了直线方程的系数 a, b, c。然后根据该障碍物实例的半径 radius 和当前点的位置 self.pos，计算了该点到直线的距离。
        如果距离大于等于半径，则表示该点与直线没有相交，直接返回 True；否则，表示该点与直线相交，返回 False。
        
        需要注意的是，这里的 self.pos 表示该障碍物实例的位置，而不是传入方法的起点或终点。
        这是因为该方法是在障碍物实例中定义的，用于判断从该障碍物实例到其他点的连线是否与该障碍物相交。
        """

        if a == b == 0 : return np.linalg.norm(start.pos-self.pos) > self.radius
        return abs(a*self.pos[0] + b*self.pos[1] + c) / (a**2 + b**2)**0.5 > self.radius

    def __eq__(self, other):
        if isinstance(other, DynamicObstacle):
            return (self.pos == other.pos).all() and self.radius == other.radius
        return False

    def __hash__(self):
        return hash((tuple(self.pos), self.radius))
